#include "FSM.h"
#include "state_passive.h"
#include "state_fall.h"
#include "state_fixstand.h"
#include "state_troting.h"

typedef enum
{
    NORMAL,
    CHANGE,
} FSMMode_e;

struct FSM_t
{
    FSMMode_e mode;
    FSMState_e current_state;
    FSMState_e next_state;

    void (*enter)(void);
    void (*run)(void);
    void (*exit)(void);
} fsm;

/* 状态切换 */
static inline void fsm_change_state()
{
    /* 当前状态为下一个状态 */
    fsm.current_state = fsm.next_state;
    /* 设置当前函数指针 */
    switch (fsm.next_state)
    {
    case PASSIVE:
        fsm.enter = passive_enter;
        fsm.run = passive_run;
        fsm.exit = passive_exit;
        break;

    case FALL:
        fsm.enter = fall_enter;
        fsm.run = fall_run;
        fsm.exit = fall_exit;
        break;

    case FIXEDSTAND:
        fsm.enter = fixedstand_enter;
        fsm.run = fixedstand_run;
        fsm.exit = fixedstand_exit;
        break;

    case TROTING:
        fsm.enter = troting_enter;
        fsm.run = troting_run;
        fsm.exit = troting_exit;
        break;
    }
}

/* 有限状态机初始化 */
inline void fsm_init(FSMState_e state)
{
    fsm.current_state = state;
    fsm.next_state = state;
    fsm_change_state();
    fsm.enter();
    fsm.mode = NORMAL;
}

/* 有限状态机运行 */
inline void fsm_update(void)
{
    if (fsm.mode == NORMAL)
    {
        fsm.run(); // In normal mode, run current state.
    }
    else if (fsm.mode == CHANGE)
    {
        fsm.exit(); // In changing mode, run the exit function for previous state,
        fsm_change_state();
        fsm.enter(); // and run the enter function for next state.
        fsm.mode = NORMAL;
        fsm.run(); // then run the run function for current state.
    }
}

/* 有限状态机设置状态 */
inline void fsm_set_state(FSMState_e state)
{
    fsm.mode = CHANGE;
    fsm.next_state = state;
}
